#include "motor_l298n.h"

#include "pid.h"
#include "pwm.h"
#include "encoder.h"

#include "stm32f10x.h"                  // Device header

void MOT_Init(MotorX *MotorX) {

	PWM_Init(MotorX->PWMx);

	CODER_Init(MotorX->EncoderX, &MotorX->code, &MotorX->speed);

	/* 马达GPIO初始化 */
	GPIO_InitTypeDef MOT_GIPOInitStructure;
	MOT_GIPOInitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	MOT_GIPOInitStructure.GPIO_Speed = GPIO_Speed_50MHz;

	if (MotorX->MOT_GPIO1 == GPIOA || MotorX->MOT_GPIO2 == GPIOA)
	{
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
	}

	if (MotorX->MOT_GPIO1 == GPIOB || MotorX->MOT_GPIO2 == GPIOB)
	{
		RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
	}

	MOT_GIPOInitStructure.GPIO_Pin = MotorX->MOT_GPIO1Pin;
	GPIO_Init(MotorX->MOT_GPIO1, &MOT_GIPOInitStructure);

	MOT_GIPOInitStructure.GPIO_Pin = MotorX->MOT_GPIO2Pin;
	GPIO_Init(MotorX->MOT_GPIO2, &MOT_GIPOInitStructure);
	
}

void MOT_On(MotorX *MotorX, float power) {

	MotorX->power = power;

	if (MotorX->power >= 0)
	{
		GPIO_SetBits(MotorX->MOT_GPIO1, MotorX->MOT_GPIO1Pin);
		GPIO_ResetBits(MotorX->MOT_GPIO2, MotorX->MOT_GPIO2Pin);
	}
	else
	{
		GPIO_SetBits(MotorX->MOT_GPIO2, MotorX->MOT_GPIO2Pin);
		GPIO_ResetBits(MotorX->MOT_GPIO1, MotorX->MOT_GPIO1Pin);
		power = -power;
	}
		
	switch (MotorX->PWMx->OCx)
	{
		case OC1: PWM_OC1SetDuty(MotorX->PWMx, power); break;
		case OC2: PWM_OC2SetDuty(MotorX->PWMx, power); break;
		case OC3: PWM_OC3SetDuty(MotorX->PWMx, power); break;
		case OC4: PWM_OC4SetDuty(MotorX->PWMx, power); break;
	}

}

void MOT_Off(MotorX *MotorX) {

	switch (MotorX->PWMx->OCx)
	{
		case OC1: PWM_OC1SetDuty(MotorX->PWMx, 0); break;
		case OC2: PWM_OC2SetDuty(MotorX->PWMx, 0); break;
		case OC3: PWM_OC3SetDuty(MotorX->PWMx, 0); break;
		case OC4: PWM_OC4SetDuty(MotorX->PWMx, 0); break;
	}
}

void MOT_Reset(MotorX *MotorX) {

	*(MotorX->code) = 0;
	*(MotorX->speed) = 0;
}
